#include "Sphere.h"


glm::vec3 Sphere::collide(glm::vec3 prev, glm::vec3 pos) {
    glm::vec3 PtoC, d, temp;
    float A, B, C, t, t1, t2, deter;

    d = pos - prev;
    PtoC = prev - center;
    A = glm::dot(d, d);
    B = 2 * glm::dot(d, PtoC);
    C = glm::dot(PtoC, PtoC) - radius * radius;

    deter = B * B - 4 * A * C;

    t1 = (-B + sqrt(deter)) / (2 * A);
    t2 = (-B - sqrt(deter)) / (2 * A);

    t = t1;
    if (t2 > 0.0001 && t2 < t1) {
        t = t2;
    }
    
    temp = prev + d * t;
    return temp - ((float) sgn(prev)) * temp *
     0.001f;
}

bool Sphere::check_collide(glm::vec3 prev, glm::vec3 pos) {
    return sgn(prev) != sgn(pos);
}

int Sphere::sgn(glm::vec3 pos) {
    return glm::length(pos - center) * glm::length(pos - center) - 
     radius * radius > 0;
}

glm::vec3 Sphere::getNormal(glm::vec3 pos) {
    return ((float) sgn(pos)) * glm::normalize(pos - center);
}
